// $Id: ViewerWidget.cpp 363 2012-04-16 09:41:51Z ahornung $

/**
 * Octomap:
 * A  probabilistic, flexible, and compact 3D mapping library for robotic systems.
 * @author K. M. Wurm, A. Hornung, University of Freiburg, Copyright (C) 2009.
 * @see http://octomap.sourceforge.net/
 * License: GNU GPL v2, http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
 */

/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#include <octovis/ViewerWidget.h>

#ifndef M_PI_2
#define M_PI_2 1.5707963267948966192E0
#endif

using namespace std;

namespace octomap {

  ViewerWidget::ViewerWidget(QWidget* parent) :
    QGLViewer(parent), m_zMin(0.0),m_zMax(1.0) {
	
    m_printoutMode = false;
    m_heightColorMode = false;      
    m_semantic_coloring = false;      
    m_drawSelectionBox = false;
  }

  void ViewerWidget::init() {

//    setHandlerKeyboardModifiers(QGLViewer::CAMERA, Qt::AltModifier);
//    setHandlerKeyboardModifiers(QGLViewer::FRAME, Qt::NoModifier);
//    setHandlerKeyboardModifiers(QGLViewer::CAMERA, Qt::ControlModifier);
    setMouseTracking(true);

    // Restore previous viewer state.
    restoreStateFromFile();

    // Make camera the default manipulated frame.
    setManipulatedFrame( camera()->frame() );


    // Light initialization:
    glEnable(GL_LIGHT0);

    float pos[4] = {-1.0, 1.0, 1.0, 0.0};
    // Directional light
    glLightfv(GL_LIGHT0, GL_POSITION, pos);

    // background color defaults to white
    this->setBackgroundColor( QColor(255,255,255) );
    this->qglClearColor( this->backgroundColor() );
  }

  void ViewerWidget::resetView(){
    this->camera()->setOrientation((float) -M_PI_2, (float) M_PI_2);
    this->showEntireScene();
    updateGL();
  } 


  QString ViewerWidget::helpString() const{
    QString help = "<h2>Octomap 3D viewer</h2>";

    help +="The Octomap library implements a 3D occupancy grid mapping approach. "
      "It provides data structures and mapping algorithms. The map is implemented "
      "using an octree. 3D maps can be viewed an built using this 3D viewer."
      "<br/><br/>"
      "Octomap is available at http://octomap.sourceforge.net/, and is actively "
      "maintained by Kai M. Wurm and Armin Hornung. This 3D viewer is based on "
      "libQGLViewer, available at http://www.libqglviewer.com/."
      "<br/><br/>"
      "Please refer to the \"Keyboard\" and \"Mouse\" tabs for instructions on "
      "how to use the viewer.";
    return help;
  }

  void ViewerWidget::enableHeightColorMode (bool enabled) {
    m_heightColorMode = enabled;
    for(std::vector<SceneObject*>::iterator it = m_sceneObjects.begin(); it != m_sceneObjects.end(); it++) {
      (*it)->enableHeightColorMode(enabled);
    }
    updateGL();
  }

  void ViewerWidget::enablePrintoutMode(bool enabled) {
    m_printoutMode = enabled;
    for(std::vector<SceneObject*>::iterator it = m_sceneObjects.begin(); it != m_sceneObjects.end(); it++) {
      (*it)->enablePrintoutMode(enabled);
    }
    updateGL();
  }

  void ViewerWidget::enableSemanticColoring (bool enabled) {
    m_semantic_coloring = enabled;
    for(std::vector<SceneObject*>::iterator it = m_sceneObjects.begin(); it != m_sceneObjects.end(); it++) {
      (*it)->enableSemanticColoring(enabled);
    }
    updateGL();
  }

  void ViewerWidget::enableSelectionBox(bool enabled) {
	  m_drawSelectionBox = enabled;
	  updateGL();
  }


  qglviewer::Quaternion ViewerWidget::poseToQGLQuaternion(const octomath::Pose6D& pose) {
    // copying octomap::Quaternion parameters to qglviewer::Quaternion does not work (reason unknown)
    // octomath::Quaternion quaternion = pose.rot().normalized();
    // return qglviewer::Quaternion(quaternion.x(), quaternion.y(), quaternion.z(), quaternion.u());

    // Compute viewing direction and use code from libqglviewer's "look at" function
    octomath::Vector3 dir = pose.rot().rotate(octomath::Vector3(1.,0.,0.));
    qglviewer::Vec direction(dir.x(), dir.y(), dir.z());
    //qglviewer::Vec xAxis = direction ^ camera()->upVector();
    // useing 0, 0, 1 as upvector instead:
    qglviewer::Vec xAxis = direction ^ qglviewer::Vec(0.0, 0.0, 1.0);

    qglviewer::Quaternion q;
    q.setFromRotatedBasis(xAxis, xAxis^direction, -direction);
    return q;
  }

  void ViewerWidget::setCamPosition(double x, double y, double z, double lookX, double lookY, double lookZ){
    this->camera()->setOrientation(-M_PI/2., M_PI/2.);
    camera()->setPosition(qglviewer::Vec(x, y, z));
    camera()->lookAt(qglviewer::Vec(lookX, lookY, lookZ));
    camera()->setUpVector(qglviewer::Vec(0.0, 0.0, 1.0));
    updateGL();
  }

  void ViewerWidget::setCamPose(const octomath::Pose6D& pose){
    octomath::Pose6D ahead = pose * octomath::Pose6D(octomath::Vector3(1,0,0), octomath::Quaternion());
    setCamPosition(pose.x(), pose.y(), pose.z(), ahead.x(), ahead.y(), ahead.z());
  }

  void ViewerWidget::jumpToCamFrame(int id, int frame) {
    qglviewer::KeyFrameInterpolator *kfi = camera()->keyFrameInterpolator(id);
    if(kfi && frame >= 0 && frame < kfi->numberOfKeyFrames()) {
      camera()->setPosition(kfi->keyFrame(frame).position());
      camera()->setOrientation(kfi->keyFrame(frame).orientation());
    } else {
      std::cerr << "Error: Could not jump to frame " << frame << " of " << kfi->numberOfKeyFrames() << std::endl;
    }
    updateGL();
  }

  void ViewerWidget::deleteCameraPath(int id) {
    if(camera()->keyFrameInterpolator(id)) {
      disconnect(camera()->keyFrameInterpolator(id), SIGNAL(interpolated()), this, SLOT(updateGL()));
      disconnect(camera()->keyFrameInterpolator(id), SIGNAL(interpolated()), this, SLOT(cameraPathInterpolated()));
      disconnect(camera()->keyFrameInterpolator(id), SIGNAL(endReached()), this, SLOT(cameraPathFinished()));
      camera()->deletePath(id);
    }
  }

  void ViewerWidget::appendToCameraPath(int id, const octomath::Pose6D& pose) {
    qglviewer::Vec position(pose.trans().x(), pose.trans().y(), pose.trans().z());
    qglviewer::Quaternion quaternion = poseToQGLQuaternion(pose);
    qglviewer::Frame frame(position, quaternion);
    if(!camera()->keyFrameInterpolator(id)) {
      camera()->setKeyFrameInterpolator(id, new qglviewer::KeyFrameInterpolator(camera()->frame()));
    }
    camera()->keyFrameInterpolator(id)->addKeyFrame(frame);
  }

  void ViewerWidget::removeFromCameraPath(int id, int frame) {
    qglviewer::KeyFrameInterpolator *old_kfi = camera()->keyFrameInterpolator(id);
    if(old_kfi) {
      qglviewer::KeyFrameInterpolator *new_kfi = new qglviewer::KeyFrameInterpolator(camera()->frame());
      for(int i = 0; i < old_kfi->numberOfKeyFrames(); i++) {
        if(i != frame) {
          new_kfi->addKeyFrame(old_kfi->keyFrame(i));
        }
      }
      deleteCameraPath(id);
      camera()->setKeyFrameInterpolator(id, new_kfi);
    }
  }

  void ViewerWidget::updateCameraPath(int id, int frame) {
    qglviewer::KeyFrameInterpolator *old_kfi = camera()->keyFrameInterpolator(id);
    if(old_kfi) {
      qglviewer::KeyFrameInterpolator *new_kfi = new qglviewer::KeyFrameInterpolator(camera()->frame());
      for(int i = 0; i < old_kfi->numberOfKeyFrames(); i++) {
        if(i != frame) {
          new_kfi->addKeyFrame(old_kfi->keyFrame(i));
        } else {
          new_kfi->addKeyFrame(*(camera()->frame()));
        }
      }
      deleteCameraPath(id);
      camera()->setKeyFrameInterpolator(id, new_kfi);
    }
  }

  void ViewerWidget::appendCurrentToCameraPath(int id) {
    int frame = 0;
    if(camera()->keyFrameInterpolator(id)) frame = camera()->keyFrameInterpolator(id)->numberOfKeyFrames();
    addCurrentToCameraPath(id, frame);
  }

  void ViewerWidget::addCurrentToCameraPath(int id, int frame) {
    qglviewer::KeyFrameInterpolator *old_kfi = camera()->keyFrameInterpolator(id);
    if(!old_kfi || frame >= old_kfi->numberOfKeyFrames()) {
      camera()->addKeyFrameToPath(id);
    } else {
      qglviewer::KeyFrameInterpolator *new_kfi = new qglviewer::KeyFrameInterpolator(camera()->frame());
      for(int i = 0; i < old_kfi->numberOfKeyFrames(); i++) {
        new_kfi->addKeyFrame(old_kfi->keyFrame(i));
        if(i == frame) {
          new_kfi->addKeyFrame(camera()->frame());
        }
      }
      deleteCameraPath(id);
      camera()->setKeyFrameInterpolator(id, new_kfi);
    }
  }

  void ViewerWidget::playCameraPath(int id, int start_frame) {
    qglviewer::KeyFrameInterpolator *kfi = camera()->keyFrameInterpolator(id);
    if(kfi && !kfi->interpolationIsStarted() && start_frame >= 0 && start_frame < kfi->numberOfKeyFrames()) {
      m_current_camera_path = id;
      m_current_camera_frame = start_frame;
      kfi->setInterpolationTime(kfi->keyFrameTime(start_frame));
      std::cout << "Playing path of length " << kfi->numberOfKeyFrames() << ", start time " << kfi->keyFrameTime(start_frame) << std::endl;
      connect(kfi, SIGNAL(interpolated()), this, SLOT(updateGL()));
      connect(kfi, SIGNAL(interpolated()), this, SLOT(cameraPathInterpolated()));
      connect(kfi, SIGNAL(endReached()), this, SLOT(cameraPathFinished()));
      kfi->startInterpolation();
    }
  }

  void ViewerWidget::stopCameraPath(int id) {
    if(camera()->keyFrameInterpolator(id) && camera()->keyFrameInterpolator(id)->interpolationIsStarted()) {
      disconnect(camera()->keyFrameInterpolator(id), SIGNAL(interpolated()), this, SLOT(updateGL()));
      disconnect(camera()->keyFrameInterpolator(id), SIGNAL(interpolated()), this, SLOT(cameraPathInterpolated()));
      disconnect(camera()->keyFrameInterpolator(id), SIGNAL(endReached()), this, SLOT(cameraPathFinished()));
      camera()->keyFrameInterpolator(id)->stopInterpolation();
    }
  }

  void ViewerWidget::cameraPathFinished() {
    if(camera()->keyFrameInterpolator(m_current_camera_path)) {
      disconnect(camera()->keyFrameInterpolator(m_current_camera_path), SIGNAL(interpolated()), this, SLOT(updateGL()));
      disconnect(camera()->keyFrameInterpolator(m_current_camera_path), SIGNAL(interpolated()), this, SLOT(cameraPathInterpolated()));
      disconnect(camera()->keyFrameInterpolator(m_current_camera_path), SIGNAL(endReached()), this, SLOT(cameraPathFinished()));
      emit cameraPathStopped(m_current_camera_path);
    }
  }

  void ViewerWidget::cameraPathInterpolated() {
    qglviewer::KeyFrameInterpolator *kfi = camera()->keyFrameInterpolator(m_current_camera_path);
    if(kfi) {
      int current_frame = m_current_camera_frame;
      for(int i = m_current_camera_frame + 1; i < kfi->numberOfKeyFrames(); i++) {
        if(kfi->keyFrameTime(current_frame) <= kfi->interpolationTime()) current_frame = i;
        else break;
      }
      if(current_frame != m_current_camera_frame) {
        m_current_camera_frame = current_frame;
        emit cameraPathFrameChanged(m_current_camera_path, m_current_camera_frame);
      }
    }
  }

  void ViewerWidget::setSceneBoundingBox(const qglviewer::Vec& min, const qglviewer::Vec& max){
    m_zMin = min[2];
    m_zMax = max[2];
    QGLViewer::setSceneBoundingBox(min, max);
  }

  void ViewerWidget::addSceneObject(SceneObject* obj){
    assert (obj);
    m_sceneObjects.push_back(obj);
    updateGL();
  }

  void ViewerWidget::removeSceneObject(SceneObject* obj){
    assert(obj);
    for(std::vector<SceneObject*>::iterator it = m_sceneObjects.begin();
        it != m_sceneObjects.end();){
      if (*it == obj)
        it = m_sceneObjects.erase(it);
      else
        ++it;
    }
    updateGL();
  }

  void ViewerWidget::clearAll(){
    // clear drawable objects:
    for(std::vector<SceneObject*>::iterator it = m_sceneObjects.begin(); it != m_sceneObjects.end(); it++){
      (*it)->clear();
    }
  }

  void ViewerWidget::draw(){

    // debugging: draw light in scene
    //drawLight(GL_LIGHT0);

    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    glEnable(GL_LIGHTING);
    if (m_printoutMode){
      glCullFace(GL_BACK);
    }

    // draw drawable objects:
    for(std::vector<SceneObject*>::iterator it = m_sceneObjects.begin(); 
        it != m_sceneObjects.end(); ++it){
      (*it)->draw();
    }

    if (m_drawSelectionBox){
    	m_selectionBox.draw();

      if (m_selectionBox.getGrabbedFrame() >= 0){
        setMouseBinding(Qt::LeftButton, FRAME, TRANSLATE);
      } else {
        setMouseBinding(Qt::LeftButton, FRAME, ROTATE);
      }

    }

  }

  void ViewerWidget::drawWithNames(){

	  if (m_drawSelectionBox)
		  m_selectionBox.draw(true);
  }

  void ViewerWidget::postDraw(){

    // Reset model view matrix to world coordinates origin
    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    camera()->loadModelViewMatrix();
    // TODO restore model loadProjectionMatrixStereo

    // Save OpenGL state
    glPushAttrib(GL_ALL_ATTRIB_BITS);

    glDisable(GL_COLOR_MATERIAL);
    qglColor(foregroundColor());

    if (gridIsDrawn()){
      glLineWidth(1.0);
      drawGrid(5.0, 10);
    }
    if (axisIsDrawn()){
      glLineWidth(2.0);
      drawAxis(1.0);
    }

    // Restore GL state
    glPopAttrib();
    glPopMatrix();

    m_drawAxis = axisIsDrawn();
    m_drawGrid = gridIsDrawn();
    setAxisIsDrawn(false);
    setGridIsDrawn(false);
    QGLViewer::postDraw();

    setAxisIsDrawn(m_drawAxis);
    setGridIsDrawn(m_drawGrid);
  }

  void ViewerWidget::postSelection(const QPoint&)
  {

  }



} // namespace

